//==================================================================
//  Copyright (C) 2006  Davide Pasca
//
//  This library is free software; you can redistribute it and/or
//  modify it under the terms of the GNU Lesser General Public
//  License as published by the Free Software Foundation; either
//  version 2.1 of the License, or (at your option) any later version.
//
//  This library is distributed in the hope that it will be useful,
//  but WITHOUT ANY WARRANTY; without even the implied warranty of
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//  Lesser General Public License for more details.
//
//  You should have received a copy of the GNU Lesser General Public
//  License along with this library; if not, write to the Free Software
//  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
//==================================================================
/// @file	pmatrix.h
///
/// 
///
//==================================================================

#ifndef PMATRIX_H
#define PMATRIX_H

#define _USE_MATH_DEFINES
#include <math.h>
#include "psys.h"
#include "pvector.h"

//==================================================================
namespace PMath
{

//==================================================================
class Matrix44_nal
{
public:
	enum Default
	{
		IDENTITY
	};

	union {
		float	_r[4][4];
		struct
		{
			float	_m11, _m12, _m13, _m14;
			float	_m21, _m22, _m23, _m24;
			float	_m31, _m32, _m33, _m34;
			float	_m41, _m42, _m43, _m44;
		};
		float	_m[4][4];
	};

	const	Vector3 *GetRow3(int i) const			{	return (const Vector3 *)_m[i];	}
			Vector3 *GetRow3(int i)					{	return (Vector3 *)_m[i];		}
	const	Vector4 *GetRow4(int i)	const			{	return (const Vector4 *)_m[i];	}
			Vector4 *GetRow4(int i)					{	return (Vector4 *)_m[i];		}

	Matrix44_nal(){}
	Matrix44_nal( Default def )
	{
		PASSERT( def == IDENTITY );
		Identity();
	}
	Matrix44_nal(	float m11, float m12, float m13, float m14,
				float m21, float m22, float m23, float m24,
				float m31, float m32, float m33, float m34,
				float m41, float m42, float m43, float m44 )
	{
		_m11 = m11, _m12 = m12, _m13 = m13, _m14 = m14;
		_m21 = m21, _m22 = m22, _m23 = m23, _m24 = m24;
		_m31 = m31, _m32 = m32, _m33 = m33, _m34 = m34;
		_m41 = m41, _m42 = m42, _m43 = m43, _m44 = m44;
	}

	Matrix44_nal(	const Vector4 &row0,
				const Vector4 &row1,
				const Vector4 &row2,
				const Vector4 &row3 )
	{
		((Vector4 *)_r)[0] = row0;
		((Vector4 *)_r)[1] = row1;
		((Vector4 *)_r)[2] = row2;
		((Vector4 *)_r)[3] = row3;
	}

	void Store( float dest[16] ) const
	{
		memcpy( dest, _r[0], sizeof(Matrix44_nal) );
	}
	void Load( const float src[16] )
	{
		memcpy( _r[0], src, sizeof(Matrix44_nal) );
	}

	void Identity()
	{
		((u_long64 *)_m)[0] = 0;	((u_long64 *)_m)[1] = 0;
		((u_long64 *)_m)[2] = 0;	((u_long64 *)_m)[3] = 0;
		((u_long64 *)_m)[4] = 0;	((u_long64 *)_m)[5] = 0;
		((u_long64 *)_m)[6] = 0;	((u_long64 *)_m)[7] = 0;
		_m11 = 1;
		_m22 = 1;
		_m33 = 1;
		_m44 = 1;
	}

	float		GetDeterminant() const;
	Matrix44_nal	GetAdjoint() const;
	Matrix44_nal	GetInverse() const;

	static Matrix44_nal	RotAxis( float ang, float ax, float ay, float az )
	{
		float	xx, yy, zz, xy, yz, zx, xs, ys, zs;

		float s = sinf( ang );
		float c = cosf( ang );

		xx = ax * ax;	yy = ay * ay;	zz = az * az;
		xy = ax * ay;	yz = ay * az;	zx = az * ax;
		xs = ax * s;	ys = ay * s;	zs = az * s;
		float one_c = 1 - c;

		return Matrix44_nal(
			(one_c * xx) + c,	(one_c * xy) + zs,	(one_c * zx) - ys,	0,
			(one_c * xy) - zs,	(one_c * yy) + c,	(one_c * yz) + xs,	0,
			(one_c * zx) + ys,	(one_c * yz) - xs,	(one_c * zz) + c,	0,
			0,					0,					0,					1 );
	}

	static Matrix44_nal	RotX( float ang )
	{
		float s = sinf( ang );
		float c = cosf( ang );
		return Matrix44_nal(
			1,	0,	0,	0,
			0,	 c,	s,	0,
			0,	-s,	c,	0,
			0,	0,	0,	1 );
	}
	static Matrix44_nal	RotY( float ang )
	{
		float s = sinf( ang );
		float c = cosf( ang );
		return Matrix44_nal(
			c,	0,	-s,	0,
			0,	1,	0,	0,
			s,	0,	c,	0,
			0,	0,	0,	1 );
	}
	static Matrix44_nal	RotZ( float ang )
	{
		float s = sinf( ang );
		float c = cosf( ang );
		return Matrix44_nal(
			c,	s,	0,	0,
			-s,	c,	0,	0,
			0,	0,	1,	0,
			0,	0,	0,	1 );
	}

	static Matrix44_nal	Tra( float tx, float ty, float tz )
	{
		return Matrix44_nal(
			1,	0,	0,	0,
			0,	1,	0,	0,
			0,	0,	1,	0,
			tx,	ty,	tz,	1 );
	}
	static Matrix44_nal	Tra( const Vector3 &tr )
	{
		return Matrix44_nal(
			1,	0,	0,	0,
			0,	1,	0,	0,
			0,	0,	1,	0,
			tr._x,	tr._y,	tr._z,	1 );
	}

	static Matrix44_nal	Sca( float sx, float sy, float sz )
	{
		return Matrix44_nal(
			sx,	0,	0,	0,
			0,	sy,	0,	0,
			0,	0,	sz,	0,
			0,	0,	0,	1 );
	}

	//==================================================================
	static Matrix44_nal PerspectiveProj( float fovy, float aspect, float znear, float zfar )
	{
		float	ang = fovy * 0.5f;
		float	f = cosf( ang ) / sinf( ang );

		return Matrix44_nal(
			f / aspect,	0,	0,	0,
			0,	f,	0,	0,
			0,	0,	(zfar+znear) / (znear-zfar),	-1,
			0,	0,	2*zfar*znear / (znear-zfar),	1 );
	}

	//=============================================================================
	static Matrix44_nal OrthogonalProj( float x1, float x2, float y2, float y1, float n, float f )
	{
		float dx = x2 - x1;
		float dy = y1 - y2;
		float dz = f - n;

		PASSERT( dx != 0 && dy != 0 && dz != 0 );

		return Matrix44_nal(
			2.0f / dx,	0,	0,	0,
			0,	2.0f / dy,	0,	0,
			0,	0,	-2.0f / dz, 0,
			
			-(x2 + x1) / dx,
			-(y1 + y2) / dy,
			-( f +  n) / dz,
			1 );
	}

	//=============================================================================
	static Matrix44_nal LookAt( const Vector4 &from, const Vector4 &center, const Vector4 &up )
	{
		Vector4	z_dir = (center - from).GetNormalized3();
		Vector4	x_dir = z_dir.GetCross3( up ).GetNormalized3();
		Vector4	y_dir = x_dir.GetCross3( z_dir ).GetNormalized3();

		return Matrix44_nal( x_dir._x,	y_dir._x,	-z_dir._x,	0,
						 x_dir._y,	y_dir._y,	-z_dir._y,	0,
						 x_dir._z,	y_dir._z,	-z_dir._z,	0,
						 0,	0,	0,	1 ) *
			   Matrix44_nal::Tra( (-from).GetVector3() );
	}

	//=============================================================================
	static Matrix44_nal LookAt( const Vector3 &from, const Vector3 &to, const Vector3 &up )
	{
		return LookAt( Vector4(from,1), Vector4(to,1), Vector4(up,0) );
	}

	//=============================================================================
	Matrix44_nal	GetAs33() const
	{
		return Matrix44_nal(	_m11, _m12, _m13, 0,
							_m21, _m22, _m23, 0,
							_m31, _m32, _m33, 0,
							0, 0, 0, 1 );
	}

	//=============================================================================
	static Matrix44_nal FromQuatern( const Quatern &q )
	{
		// calculate coefficients
		float x2 = q._v3._x + q._v3._x;
		float y2 = q._v3._y + q._v3._y;
		float z2 = q._v3._z + q._v3._z;
		float xx = q._v3._x * x2;
		float xy = q._v3._x * y2;
		float xz = q._v3._x * z2;
		float yy = q._v3._y * y2;
		float yz = q._v3._y * z2;
		float zz = q._v3._z * z2;
		float wx = q._w * x2;
		float wy = q._w * y2;
		float wz = q._w * z2;

		Matrix44_nal	m( Matrix44_nal::IDENTITY );

		m._m[0][0] = 1.0f - (yy + zz);
		m._m[1][0] = xy - wz;
		m._m[2][0] = xz + wy;

		m._m[0][1] = xy + wz;
		m._m[1][1] = 1.0f - (xx + zz);
		m._m[2][1] = yz - wx;

		m._m[0][2] = xz - wy;
		m._m[1][2] = yz + wx;
		m._m[2][2] = 1.0f - (xx + yy);

		return m;
	}

	Quatern GetQuaternion() const;

	void	 BackTransform();
	Matrix44_nal GetBackTransformed() const
	{
		Matrix44_nal newm = *this;
		newm.BackTransform();
		return newm;
	}

	Matrix44_nal GetBackProjectedPersp() const;
	Matrix44_nal Transpose() const;
	Matrix44_nal Transpose33() const;

	//void SetOrthogonalProjInv( float x1, float x2, float y2, float y1, float n, float f );

	void SetPerspectiveProj( float fovy, float aspect, float znear, float zfar );

	void CopyFromOGL( const double ogl_mat[16] );
	void CopyFromOGL( const float ogl_mat[16] );

	const float *GetArray() const
	{
		return _m[0];
	}

	//=============================================================================
	PFORCEINLINE Matrix44_nal operator *( const Matrix44_nal &m2 ) const
	{
		Matrix44_nal	mout;

		float	*outp = (float *)mout._m;
		float	*inp = (float *)m2._m;
		for(int j=0; j < 4; ++j)
		{
			float	x = *inp++;
			float	y = *inp++;
			float	z = *inp++;
			float	w = *inp++;
			*outp++ = _m[0][0] * x + _m[1][0] * y + _m[2][0] * z + _m[3][0] * w;
			*outp++ = _m[0][1] * x + _m[1][1] * y + _m[2][1] * z + _m[3][1] * w;
			*outp++ = _m[0][2] * x + _m[1][2] * y + _m[2][2] * z + _m[3][2] * w;
			*outp++ = _m[0][3] * x + _m[1][3] * y + _m[2][3] * z + _m[3][3] * w;
		}

		return mout;
	}

	//=============================================================================
	PFORCEINLINE void operator *=( const Matrix44_nal &m2 )
	{
		*this = *this * m2;
	}

	//friend Matrix44_nal operator * ( const Matrix44_nal &m1, const Matrix44_nal &m2 );
};

//==================================================================
typedef Matrix44_nal	__declspec(align(16)) Matrix44;

//==================================================================
inline Vector4 operator * ( const Matrix44 &a, const Vector4 &v )
{
	float	x = v._v[0], y = v._v[1], z = v._v[2], w = v._v[3];

	return Vector4( a._m[0][0] * x + a._m[1][0] * y + a._m[2][0] * z + a._m[3][0] * w,
					a._m[0][1] * x + a._m[1][1] * y + a._m[2][1] * z + a._m[3][1] * w,
					a._m[0][2] * x + a._m[1][2] * y + a._m[2][2] * z + a._m[3][2] * w,
					a._m[0][3] * x + a._m[1][3] * y + a._m[2][3] * z + a._m[3][3] * w );
}

//==================================================================
inline Vector3 operator * ( const Matrix44 &a, const Vector3 &v )
{
	float	x = v._v[0], y = v._v[1], z = v._v[2];

	return Vector3( a._m[0][0] * x + a._m[1][0] * y + a._m[2][0] * z + a._m[3][0],
					a._m[0][1] * x + a._m[1][1] * y + a._m[2][1] * z + a._m[3][1],
					a._m[0][2] * x + a._m[1][2] * y + a._m[2][2] * z + a._m[3][2] );
}

//==================================================================
inline Vector4 operator * ( const Vector4 &v, const Matrix44 &a )
{
	float	x = v._v[0], y = v._v[1], z = v._v[2], w = v._v[3];

	return Vector4( x * a._m[0][0] + y * a._m[0][1] + z * a._m[0][2] + w * a._m[0][3],
					x * a._m[1][0] + y * a._m[1][1] + z * a._m[1][2] + w * a._m[1][3],
					x * a._m[2][0] + y * a._m[2][1] + z * a._m[2][2] + w * a._m[2][3],
					x * a._m[3][0] + y * a._m[3][1] + z * a._m[3][2] + w * a._m[3][3] );
}


//==================================================================
};

#endif
